/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package modes;

import java.io.File;
import lejos.nxt.Sound;
import navigation.Navigator;
import main.ColorSensorChew;
import main.Engine;
import main.SensorEngine;
import main.SonicSensor;
import navigation.Direction;
import navigation.GoRightExitNavigator;
import navigation.IllegalMapPositionException;
import navigation.IllegalMapStateException;
import navigation.IllegalMoveException;

/**
 *If reads yellow
 * @author Max
 */
public class ModeYellow extends AMode
{

    private final int MAX_COUNT_WHITE = 40;
    private int currentMaxWhite = MAX_COUNT_WHITE / 2;
    private int countYellow = 0;
    private int countWhite = 0;
    private int failureCount = 0;
    private int dir = -1;

    /**
     *
     * @param en Engine
     * @param r SensorEngine
     * @param c ColorSensor
     * @param s SonicSensor
     * @param nav Navigator
     */
    public ModeYellow(Engine en, SensorEngine r, ColorSensorChew c, SonicSensor s, Navigator nav)
    {
        super(en, r, c, s, nav);

    }

    /**
     * If field is unknown, he checks the directions
     * Turns in a direction
     *
     * @post read black
     * @param withoutCheck
     * @return neuen Modus
     * @throws InterruptedException
     */
    public Mode run(boolean withoutCheck)
            throws InterruptedException
    {
        Mode mode = Mode.YELLOW;
        engine.stop();
        dir = -1;

        if (withoutCheck || isReadMoreThanOnce())
        {
            Sound.playSample(new File("chewy1.wav"), 100);
            moveOneStep();
            if (isCrossNotFree())
            {
                return Mode.FOUND_OBJECT;
            }

            if (!withoutCheck)
            {
                turnBackToLine();
            }
            ModeController.dm.echo("CrossRoad3Dir");

            if (!navi.fieldIsKnown())
            {
                Sound.beep();
                addField(withoutCheck);
            } else
            {
                if (navi.fieldIsExit())
                {
                    navi.directionChanged(Direction.BACKWARD);
                    moveOneStep();
                    navi.directionChanged(Direction.BACKWARD);
                    dir = -2;
                    ModeController.dm.echo("EXIT");
                }
                chooseDirection();
                findNewWay(mode, 200);
            }
            if (dir != -2)
            {
                turn();
                mode = setNewDirection(mode);
                rotationSensor.setRotating(true);
                mode = runTillBlack(mode);
                if (isAfterCrossNotFree())
                {
                    mode = Mode.FOUND_OBJECT;
                }
            } else
            {
                mode = Mode.DEAD_END;
            }

            resetYellow();
            countYellow = 0;
        } else
        {
            mode = Mode.BLACK;
        }
        return mode;
    }

    private void actionYellow()
    {
        rotationSensor.setRotating(true);
        resetYellow();
        countYellow++;
        if (rotationSensor.isLeft())
        {
            engine.driveLeft(70);
        } else if (rotationSensor.isRight())
        {
            engine.driveRight(70);
        } else
        {
            engine.forward();
        }
    }

    private void turn()
            throws InterruptedException
    {
        engine.forward();

        try
        {
            if (navi.decideDirection() == Direction.RIGHT)
            {
                rotationSensor.rotate(90);
                engine.setSpeed(600);
                Thread.sleep(800);
                rotationSensor.setRotating(false);
                engine.rotateRight();
                Thread.sleep(50);
            } else if (navi.decideDirection() == Direction.LEFT)
            {
                rotationSensor.rotate(-90);
                engine.setSpeed(600);
                Thread.sleep(600);
                rotationSensor.setRotating(false);
                engine.rotateLeft();
                Thread.sleep(50);
            } else if (navi.decideDirection() == Direction.BACKWARD)
            {
                rotationSensor.setRotating(false);
                engine.rotate180();
            } else
            {
                rotationSensor.setRotating(true);
                Thread.sleep(50);
            }
        } catch (IllegalMapStateException ex)
        {
            ModeController.dm.echo(ex.toString());
            navi = new GoRightExitNavigator(navi);

        }
        rotationSensor.rotateMiddle();
    }

    private void actionWhite()
            throws InterruptedException
    {
        if (rotationSensor.isMiddle())
        {
            rotationSensor.setRotating(false);
            if (countWhite == 0)
            {
                engine.switchRotation();
                failureCount++;
            } else if (countWhite >= currentMaxWhite)
            {
                engine.switchRotation();
                countWhite = 0;
                failureCount++;
                failureMode();
            }
            countWhite += 1;
        } else if (rotationSensor.isLeft())
        {
            engine.driveRight(70);
        } else if (rotationSensor.isRight())
        {
            engine.driveLeft(70);
        }


    }

    private void resetYellow()
    {
        countWhite = 0;
        currentMaxWhite = 50;
        failureCount = 0;
    }

    private void failureMode()
    {
        if (failureCount > 2)
        {
            currentMaxWhite += 25;
        } else
        {
            currentMaxWhite = 100;
        }
    }

    private void chooseDirection()
            throws InterruptedException
    {

        try
        {
            if (navi.decideDirection() == Direction.RIGHT)
            {
                rotationSensor.rotate(90);
                rotationSensor.setRotating(false);
            } else if (navi.decideDirection() == Direction.LEFT)
            {
                rotationSensor.setRotating(false);
                rotationSensor.rotate(-90);
            }
        } catch (IllegalMapStateException ex)
        {
            ModeController.dm.echo(ex.toString());
            engine.stop();
            Sound.playSample(new File("chewy1.wav"), 100);
            Thread.sleep(500);
            Sound.playSample(new File("chewy1.wav"), 100);
        }
        engine.forward();
    }

    private Mode findNewWay(Mode mode, int count)
            throws InterruptedException
    {
        for (int i = 0; i < count; ++i)
        {
            rotationSensor.rotateColorSensor();
            color.readColor();
            if (color.isColorBlack())
            {
                actionYellow();
                mode = Mode.BLACK;
                break;

            } else if (color.isColorYellow())
            {
                actionYellow();
                mode = Mode.YELLOW;
                break;
            }

        }
        return mode;
    }

    private void addField(boolean without)
            throws InterruptedException
    {
        rotationSensor.rotate(90);

        boolean forward = false, left = false, right = false;
        findRightLine();
        if (dir == -1)
        {
            ModeController.dm.echo("Drive-Forward");
            forward = true;
            left = true;
            rotationSensor.rotateMiddle();
            if (without)
            {
                if (!isMiddle())
                {
                    dir = -2;
                    navi.directionChanged(Direction.BACKWARD);
                    moveOneStep();
                    navi.directionChanged(Direction.BACKWARD);
                    return;
                }
            }
        } else if (isMiddle())
        {
            right = true;
            forward = true;
            ModeController.dm.echo("Drive-Right");

        } else
        {

            left = true;
            right = true;
            engine.rotateRight();
            for (int i = 0; i < 50; ++i)
            {
                color.readColor();
            }

        }
        try
        {
            navi.addField(forward, right, true, left);
        } catch (IllegalMapPositionException ex)
        {
            ModeController.dm.echo(ex.toString());
        }
        engine.backward();
        Thread.sleep(500);
        engine.stop();
    }

    private boolean isMiddle() throws InterruptedException
    {
        boolean middle = false;
        rotationSensor.rotateMiddle();
        engine.forward();
        Thread.sleep(200);
        engine.rotateRight();
        for (int j = 0; (j < MAX_COUNT_WHITE) && !middle; ++j)
        {
            color.readColor();
            if (color.isColorBlack() || color.isColorYellow())
            {
                middle = true;
                engine.stop();
                break;
            }
        }
        if (!middle)
        {
            engine.rotateLeft();
            for (int j = 0; (j < MAX_COUNT_WHITE * 2) && (!middle); ++j)
            {
                color.readColor();
                if (color.isColorBlack() || color.isColorYellow())
                {
                    middle = true;
                    engine.stop();
                    break;
                }
            }
        }
        return middle;
    }

    private boolean isReadMoreThanOnce()
    {
        boolean readMoreThanOnce = true;
        engine.stop();
        engine.rotateRight();

        for (int i = 0; i < MAX_COUNT_WHITE / 2; ++i)
        {
            color.readColor();
            if (color.isColorBlack())
            {
                engine.stop();
                readMoreThanOnce = false;
                break;
            }
        }
        if (readMoreThanOnce)
        {
            engine.rotateLeft();
            for (int i = 0; i < MAX_COUNT_WHITE; ++i)
            {
                color.readColor();
                if (color.isColorBlack())
                {
                    engine.stop();
                    readMoreThanOnce = false;
                    break;
                }
            }
        }
        return readMoreThanOnce;

    }

    private Mode runTillBlack(Mode mode)
            throws InterruptedException
    {
        while (mode == Mode.YELLOW)
        {

            rotationSensor.rotateColorSensor();
            color.readColor();

            if (color.isColorYellow())
            {
                actionYellow();
            } else if (color.isColorBlack())
            {
                actionYellow();
                mode = Mode.BLACK;
            } else
            {
                if (!color.isColorWhite())
                {
                    continue;
                }
                actionWhite();
            }
        }
        return mode;
    }

    private void moveOneStep()
    {
        try
        {
            navi.moveOneStep();
        } catch (IllegalMoveException ex)
        {
            ModeController.dm.echo(ex.toString());
        }
    }

    private Mode setNewDirection(Mode mode) throws InterruptedException
    {
        try
        {

            if (navi.decideDirection() != Direction.FORWARD && navi.decideDirection() != Direction.BACKWARD)
            {
                mode = findNewWay(mode, 3000);
            }
            if (navi.decideDirection() == Direction.RIGHT)
            {
                navi.directionChanged(Direction.RIGHT);
            } else if (navi.decideDirection() == Direction.LEFT)
            {
                navi.directionChanged(Direction.LEFT);

            } else if (navi.decideDirection() == Direction.BACKWARD)
            {
                engine.backward();
                engine.setSpeed(600);
                Thread.sleep(800);
                navi.directionChanged(Direction.BACKWARD);
            }
        } catch (IllegalMapStateException ex)
        {
            ModeController.dm.echo(ex.toString());
            engine.stop();
            Sound.playSample(new File("chewy1.wav"), 100);
            Thread.sleep(500);
            Sound.playSample(new File("chewy1.wav"), 100);

        }
        return mode;
    }

    private void turnBackToLine()
    {
        engine.rotateRight();
        for (int i = 0; i < MAX_COUNT_WHITE / 2 + 5; ++i)
        {
            color.readColor();
        }
        engine.stop();
    }

    private void findRightLine()
    {
        engine.forward();
        for (int i = 0; i < 200; ++i)
        {
            color.readColor();
            if (color.isColorYellow())
            {
                dir = 0;
                break;
            }
            if (color.isColorBlack())
            {
                dir = 0;
                break;
            }
        }
        engine.stop();
    }
}
